import imp

from matplotlib.pyplot import twinx
import rospy
import rospkg
import rostopic
import roslib
import sys
sys.path.append("/home/marcus/tutorials_ws/src/rovrqt/resource")
from qt_gui.plugin import Plugin
from PyQt5.QtWidgets import QWidget
from rov_console import Ui_Form
from cv_bridge import CvBridge
from PyQt5.QtGui import QPixmap,QImage
from rovmsg.msg import rovmsg
import numpy as np

br = CvBridge()



class Console(Ui_Form,QWidget):
    def __init__(self):
        super(Console, self).__init__()
        self.setupUi(self)
        # self.graphicsView.
        self.mysub = {}
        self.pB_change.clicked.connect(self.setup_cam)
        self.mysub["cmd"] = self.sub_topic("/ROV/cmd_vel",self.cmd_callback)
        pass
    
    def sub_topic(self,topic,callback):
        topic_type, real_topic, fields = rostopic.get_topic_type(topic,callback)        
        data_class = roslib.message.get_message_class(topic_type)
        return rospy.Subscriber(real_topic, data_class, callback)
	

    def setup_cam(self):
        rospy.loginfo("binding to camera")
        self.lb_camstate.setText("相机：已连接")
        topic_type, real_topic, fields = rostopic.get_topic_type("/usb_cam/image_raw/compressed")        
        data_class = roslib.message.get_message_class(topic_type)
        # 订阅了一个话题
        self.mysub["cam"] = rospy.Subscriber(real_topic, data_class, self.cam_callback)
        rospy.loginfo(data_class)
        
	# 话题的回调函数
    def cam_callback(self, msg):
        gFrame = br.compressed_imgmsg_to_cv2(msg, "rgb8")
        img = gFrame.astype("uint8")
        img = QImage(img[:], img.shape[1], img.shape[0], img.shape[1] * 3,QImage.Format_RGB888)
        self.label_cam.setPixmap(QPixmap(img).scaledToWidth(self.label_cam.width()))
        pass

    def cmd_callback(self,msg:rovmsg):
        self.lb_robostate.setText("机器人：已连接")
        x = msg.twist.linear.x
        y = msg.twist.linear.y
        mode_ls = ["normal","depth hold","sports","invalid"]
        speedh = np.sqrt(x*x+y*y)
        speedv = msg.twist.linear.z
        self.lb_speednumh.setText("{:.2f}".format(speedh))
        self.lb_speednumv.setText("{:.2f}".format(speedv))

        self.lb_modenum.setText(mode_ls[msg.mode-1])
        self.lb_lightnum.setText("{}".format(msg.light))
       
        self.lb_compassnum.y1 = 90-88*msg.twist.linear.x/(4*1.413)
        self.lb_compassnum.x1 = 90-88*msg.twist.linear.y/(4*1.413)
        self.lb_compassnum.update()
        pass

    


class MyPlugin(Plugin):

    def __init__(self, context):
        super(MyPlugin, self).__init__(context)
        # Give QObjects reasonable names
        self.setObjectName('MyPlugin')

        # Process standalone plugin command-line arguments
        from argparse import ArgumentParser
        parser = ArgumentParser()
        # Add argument(s) to the parser.
        parser.add_argument("-q", "--quiet", action="store_true",
                      dest="quiet",
                      help="Put plugin in silent mode")
        args, unknowns = parser.parse_known_args(context.argv())
        if not args.quiet:
            print('arguments: ', args)
            print('unknowns: ', unknowns)
            # self.setStyleSheet("background-color: rgb(54, 59, 64);\n"
            #                      "color: rgb(222, 222, 222);")

        self._widget = Console()
        # Get path to UI file which should be in the "resource" folder of this package
        # ui_file = os.path.join(rospkg.RosPack().get_path('rovrqt'), 'resource', 'rov_console.ui')
        # loadUi(ui_file, self._widget)
        
        if context.serial_number() > 1:
            self._widget.setWindowTitle(self._widget.windowTitle() + (' (%d)' % context.serial_number()))
        context.add_widget(self._widget)

    def shutdown_plugin(self):
        # TODO unregister all publishers here
        pass

    def save_settings(self, plugin_settings, instance_settings):
        # TODO save intrinsic configuration, usually using:
        # instance_settings.set_value(k, v)
        pass

    def restore_settings(self, plugin_settings, instance_settings):
        # TODO restore intrinsic configuration, usually using:
        # v = instance_settings.value(k)
        pass
